#include "videocapture.h"
#include "glutproject.h"
#include "extension/colors.h"
#include "line.h"
#include "benchmarking.h"
#include "bmath.h"
#include "rectangle.h"
#include <GL/gl.h>
#include <GL/glut.h>

#include <iostream>
#include <sstream>
#include <iomanip>
#include <vector>
#include <fstream>
#include <map>

#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netdb.h>
#include <cstdio>
#include <cstring>

#include "wheeldata.h"

using namespace bend2d;
using namespace std;

#define TRACE(x) cout << dec <<  __PRETTY_FUNCTION__ << " | " << x << "\n"

class myVideoCapture : public GlutProject, public videoCapture
{
    private:
        Point m_SpeedMarker;
        Point m_LastSpeedMarker;
        Point m_CenterMarker;
        Point m_LastCenterMarker;
        Point m_Mouse;
        int m_SpeedMarkerFoundOnFrame;
        int m_CenterMarkerFoundOnFrame;
        vector<Color> m_SpeedColor;
        vector<Color> m_CenterColor;
        double m_DetectedRadius;
        double m_LastDetectedRadius;
        Angle m_DetectedAngle;
        Angle m_LastDetectedAngle;
        double m_RelativeMovement;
        double m_AbsoluteMovement;
        int m_LastDetectionTime;
        
        int m_Frames;
        int m_FramesPerSecond;
        StopWatch m_Watch;

        bool m_Running;
        string m_Mode;
        string m_ModeDesc;

        vector<int> m_CentersToCheck;
        vector<int> m_SpeedsToCheck;

        int m_FilterRegionSize;
        int m_FilterAnalyzeRadius;
        int m_ColorThreshHold;

        bool m_ShowInfo;
        bool m_Normalize;

        bool m_NormalizeColors;
        unsigned char m_NumberOfColors;

        bool m_ShowImage;
        StopWatch m_RunTime;

        map<int, Color> m_GhostMap;
        Rectangle m_AnalysisRect;

        bool m_Logging;

        double m_Speed;
        double m_Break;

    public:
        myVideoCapture()
        {
            LoadDefaultProperties();
            
            m_Watch.Start();
            m_RunTime.Start();
            m_SpeedMarkerFoundOnFrame = 0;
            m_CenterMarkerFoundOnFrame = 0;
            m_Running = true;
            m_RelativeMovement = 0;
            m_AbsoluteMovement = 0;
            m_Logging = false;

            m_Speed = 0;
            m_Break = 0;
        }

        void LoadDefaultProperties()
        {
            m_FilterRegionSize = 50;
            m_FilterAnalyzeRadius = 7;
            m_ColorThreshHold = 20;
            m_ShowInfo = true;
            m_Normalize = false;
            m_NormalizeColors = false;
            m_NumberOfColors = 20;
            m_DetectedRadius = 0;
            m_LastDetectedRadius = 0;
            m_ShowImage = true;
            m_Frames = 0;
            m_FramesPerSecond = 0;
            m_AnalysisRect = Rectangle(80 + 40, 0 + 40, 640 - 80 - 40, 480 - 40);
        }

        void LoadProperties()
        {

        }

        void LogStatus(bool logging)
        {
            if(logging)
            {
                static string filename = "output." + GetDeviceName() + ".log";
                static ofstream output(filename.c_str());
                static bool header = false;
                if(output.good())
                {
                    if(header == false)
                    {
                        output << "Tempo(ms),Tempo do Frame Corrente(ms),Frames por Segundo,Ambos Artefatos Encontrados(total),Artefatos de Velocidade(total),Artefatos do Raio(total),"
                            "Raio(pixels),Angulo(graus),Tamanho do Filtro,Raio de analise,Tolerância da Cor,Velocidade\n";
                        header = true;
                    }
                    output << m_RunTime.GetTime() << "," << m_Watch.GetTime() << "," << m_FramesPerSecond << ","<< (m_SpeedMarkerFoundOnFrame + m_CenterMarkerFoundOnFrame) << ","
                        << m_SpeedMarkerFoundOnFrame << "," << m_CenterMarkerFoundOnFrame << "," << m_DetectedRadius << ","<< m_DetectedAngle.GetAngle(Angle::eAngleDegreePos) << ","
                        << m_FilterRegionSize << "," << m_FilterAnalyzeRadius << "," << m_ColorThreshHold << "," << m_Speed << ",\n" << flush;
                }
            }
        }

        void ExportInfo()
        {
            static bool init = false;
            static bool ok = false;
            static struct sockaddr_in server;
            static int sock = -1;

            if(init == false)
            {
                init = true;
                sock = socket(AF_INET, SOCK_DGRAM, 0);
                if(sock >= 0)
                {
                    struct hostent *hp;
                    hp = gethostbyname("127.0.0.1");

                    bzero(&server, sizeof(server));
                    server.sin_family = AF_INET;
                    server.sin_port = htons(8100);
                    server.sin_addr = *((struct in_addr *)hp->h_addr);
                    ok = true;
                }
                else
                {
                    TRACE("Error creating socket");
                }
            }

            if(ok == true)
            {
                int serverlen = sizeof(server);

                WheelData wheeldata;
                strncpy(wheeldata.m_Sender, GetDeviceName().c_str(), 20);
                wheeldata.m_Time = m_RunTime.GetTime();
                wheeldata.m_Speed = m_Speed;
                
                sendto(sock, &wheeldata, sizeof(wheeldata), 0, (sockaddr*)&server, serverlen);
                if(recvfrom(sock, &wheeldata, sizeof(wheeldata), MSG_DONTWAIT, (struct sockaddr *)&server, (socklen_t*)&serverlen) > 0)
                {
                    if(strcmp(wheeldata.m_Sender, GetDeviceName().c_str()) == 0)
                        m_Break = wheeldata.m_Break;
                }
            }
            
        }

        void MagicColor()
        {
            if(m_ShowImage)
            {
                static double color = 1;
                double vcolor = (Math::Sin(color) + 1) / 2;
                glColor3f(vcolor, vcolor, vcolor);
                color += 1;
            }
            else
            {
                glColor3f(1,1,1);
            }
        }

        void CustomRenderFunction()
        {
            static StopWatch watch;

            glRasterPos3f(0,0,0);
            
            if(m_ShowImage)
            {
                glDrawPixels(640, 480, GL_RGBA, GL_UNSIGNED_INT_8_8_8_8, getBuffers()->cbuff);
            }

            ShowFramesLayer(true);
            ShowDevelopedByLayer(true);
            ShowMenuLayer(m_ShowInfo);
            ShowInfoLayer(m_ShowInfo);
            ShowMarkers(true);
        }

        void CleanUp()
        {
            m_SpeedsToCheck.clear();
            m_CentersToCheck.clear();
            m_Speed = 0;
        }

        void CustomKeyboardFunction(unsigned char key, int x, int y)
        {
            switch(key)
            {
                case '1':
                    m_Mode = "CENTER";
                    m_ModeDesc = "Definir cor do marcador central";
                    break;
                case '2':
                    m_Mode = "SPEED";
                    m_ModeDesc = "Definir cor do marcador posicional";
                    break;
                case '3':
                    ++m_FilterRegionSize;
                    if(m_FilterRegionSize > 100)
                        m_FilterRegionSize = 100;
                    break;
                case '4':
                    --m_FilterRegionSize;
                    if(m_FilterRegionSize < 1)
                        m_FilterRegionSize = 1;
                    break;
                case '5':
                    ++m_ColorThreshHold;
                    if(m_ColorThreshHold > 0x80)
                        m_ColorThreshHold = 0x80;
                    break;
                case '6':
                    --m_ColorThreshHold;
                    if(m_ColorThreshHold < 1)
                        m_ColorThreshHold = 1;
                    break;
                case '7':
                    m_ShowInfo = !m_ShowInfo;
                    break;
                case '8':
                    m_Normalize = !m_Normalize;
                    m_NormalizeColors = false; 
                    break;
                case '9':
                    m_ShowImage = !m_ShowImage;
                    break;
                case 'a':
                    m_NormalizeColors = !m_NormalizeColors;
                    m_Normalize = false;
                    break;
                case 'q':
                    ++m_NumberOfColors;
                    break;
                case 'w':
                    --m_NumberOfColors;
                    break;
                case 'l':
                    m_Logging = !m_Logging;
                    break;
                case 'm':
                    if(m_Mode == "CAPTURE")
                    {
                        m_Mode = "";
                        m_ModeDesc = "";
                    }
                    else if(m_Mode == "")
                    {
                        m_Mode = "CAPTURE";
                        m_ModeDesc = "Mover regiao de captura";
                    }
                    break;
                case '+':
                    if(m_AnalysisRect.GetHeight() < 480)
                        m_AnalysisRect += 10;
                    break;
                case '-':
                    if(m_AnalysisRect.GetHeight() > 20)
                        m_AnalysisRect += -10;
                    break;
                case 'r':
                    LoadDefaultProperties();
                    break;
                default:
                    m_Mode = "";
                    m_ModeDesc = "";
            }
        }

        void ConvertToXY(int i, int& x, int& y)
        {
            x = i % 640;
            y = i / 640;
        }

        void ShowMarkers(bool show)
        {
            if(show)
            {
                int x, y;
                glPushMatrix();
                glBegin(GL_LINES);
                glColor3f(1,1,1);
                glLineWidth(1);
                for(int i = 0; i < m_CentersToCheck.size(); ++i)
                {
                    ConvertToXY(m_CentersToCheck[i], x, y);
                    glVertex2d(x - 3, y);
                    glVertex2d(x + 3, y);
                    glVertex2d(x, y - 3);
                    glVertex2d(x, y + 3);
                }

                for(int i = 0; i < m_SpeedsToCheck.size(); ++i)
                {
                    ConvertToXY(m_SpeedsToCheck[i], x, y);
                    glVertex2d(x - 3, y - 3);
                    glVertex2d(x + 3, y + 3);
                    glVertex2d(x - 3, y + 3);
                    glVertex2d(x + 3, y - 3);
                }
                glEnd();

                glLineWidth(3);
                glBegin(GL_LINE_LOOP);
                glColor3f(1, 0, 0);
                glVertex2d(m_SpeedMarker.GetX() - 5, m_SpeedMarker.GetY() - 5);
                glVertex2d(m_SpeedMarker.GetX() + 5, m_SpeedMarker.GetY() - 5);
                glVertex2d(m_SpeedMarker.GetX() + 5, m_SpeedMarker.GetY() + 5);
                glVertex2d(m_SpeedMarker.GetX() - 5, m_SpeedMarker.GetY() + 5);
                glEnd();

                glBegin(GL_LINE_LOOP);
                glColor3f(0, 0, 1);
                glVertex2d(m_CenterMarker.GetX() - 5, m_CenterMarker.GetY() - 5);
                glVertex2d(m_CenterMarker.GetX() + 5, m_CenterMarker.GetY() - 5);
                glVertex2d(m_CenterMarker.GetX() + 5, m_CenterMarker.GetY() + 5);
                glVertex2d(m_CenterMarker.GetX() - 5, m_CenterMarker.GetY() + 5);
                glEnd();

                glBegin(GL_LINES);
                glColor3f(1, 1, 1);
                glVertex2d(m_SpeedMarker.GetX(), m_SpeedMarker.GetY());
                glVertex2d(m_CenterMarker.GetX(), m_CenterMarker.GetY());
                glEnd();
                
                glLineWidth(2);
                glColor3f(1,1,1);
                glBegin(GL_LINE_LOOP);
                glVertex2d(m_AnalysisRect.GetLeft(), m_AnalysisRect.GetTop());
                glVertex2d(m_AnalysisRect.GetRight(), m_AnalysisRect.GetTop());
                glVertex2d(m_AnalysisRect.GetRight(), m_AnalysisRect.GetBottom());
                glVertex2d(m_AnalysisRect.GetLeft(), m_AnalysisRect.GetBottom());
                glEnd();

                
                // Speed and Break
                glLineWidth(2);
                glColor3f(1,1,1);
                glBegin(GL_LINE_LOOP);
                glVertex2d(575, 100);
                glVertex2d(595, 100);
                glVertex2d(595, 50);
                glVertex2d(575, 50);
                glEnd();
 
                glBegin(GL_LINE_LOOP);
                glVertex2d(605, 100);
                glVertex2d(625, 100);
                glVertex2d(625, 50);
                glVertex2d(605, 50);
                glEnd();

                DrawText(575, 102, "Vel");
                DrawText(605, 102, "Fre");
              
                const double speedadjust = 4.8;
                glColor3f(0, 1, 0);
                glBegin(GL_POLYGON);
                glVertex2d(576, 51);
                glVertex2d(594, 51);
                glVertex2d(594, 51 + speedadjust * Math::Abs(m_Speed));
                glVertex2d(576, 51 + speedadjust * Math::Abs(m_Speed));
                glEnd();
 
                const double breakadjust = 4.8;
                glColor3f(1, 0, 0);
                glBegin(GL_POLYGON);
                glVertex2d(606, 51);
                glVertex2d(624, 51);
                glVertex2d(624, 51 + breakadjust * Math::Abs(m_Break));
                glVertex2d(606, 51 + breakadjust * Math::Abs(m_Break));
                glEnd();


                glPopMatrix();
           }
        }

        void ShowFramesLayer(bool show)
        {
            if(show)
            {
                if(m_Watch.GetTime() > 1000)
                {
                    m_FramesPerSecond = m_Frames;
                    m_Frames = 0;
                    m_Watch.Reset();
                }
                else
                {
                    ++m_Frames;
                }

                stringstream timestr;
                timestr << m_FramesPerSecond << " FPS";
                MagicColor();
                DrawText(10,10,timestr.str());
            }
        }

        void ShowDevelopedByLayer(bool show)
        {
            if(show)
            {
                MagicColor();
                DrawText(550, 10, "por Roberto Bender");
            }
        }

        void ShowMenuLayer(bool show)
        {
            if(show)
            {
                MagicColor();
                DrawText(460, 470, "Opcoes:");
                DrawText(460, 460, "1) Definir cor do marcador central");
                DrawText(460, 450, "2) Definir cor do marcador posicional");
                DrawText(460, 440, "3) Aumentar tamanho do filtro");
                DrawText(460, 430, "4) Diminuir tamanho do filtro");
                DrawText(460, 420, "5) Aumentar tolerancia da cor");
                DrawText(460, 410, "6) Diminuir tolerancia da cor");
                DrawText(460, 400, "7) Ligar/desligar informacoes");
                DrawText(460, 390, "8) Ligar/desligar normalizacao");
                DrawText(460, 380, "9) Ligar/desligar imagem");
                DrawText(460, 370, "A) Ligar/desligar normalizacao multi cor");
                DrawText(460, 360, "Q) Aumentar o numero de cores");
                DrawText(460, 350, "W) Diminuir o numero de cores");
                DrawText(460, 340, "L) Ligar/desligar logs");
                DrawText(460, 330, "M) Difinir regiao de captura");
                DrawText(460, 320, "+) Aumentar regiao de captura");
                DrawText(460, 310, "-) Diminuir regiao de captura");
                DrawText(460, 300, "Modo: " + m_ModeDesc);
            }
        }

        void ShowInfoLayer(bool show)
        {
            if(show)
            {
                stringstream centercolorstr;
                stringstream speedcolorstr;
                stringstream centermarkerstr;
                stringstream speedmarkerstr;
                stringstream mouseposstr;
                stringstream colorthresholdstr;
                stringstream regionsizestr;
                stringstream numberofcolorsstr;
                stringstream detectedradiusstr;
                stringstream detectedanglestr;
                stringstream detectionstatusstr;
                stringstream absolutemovementstr;
                stringstream relativemovementstr;

                //centercolorstr << setw(8) << setfill('0') << hex << m_CenterColor.To8888();
                //speedcolorstr << setw(8) << setfill('0') << hex << m_SpeedColor.To8888();
                centermarkerstr << "(" << m_CenterMarker.GetX() << ", " << m_CenterMarker.GetY() << ")";
                speedmarkerstr << "(" << m_SpeedMarker.GetX() << ", " << m_SpeedMarker.GetY() << ")";
                mouseposstr << "(" << m_Mouse.GetX() << ", " << m_Mouse.GetY() << ")";
                colorthresholdstr << m_ColorThreshHold;
                regionsizestr << m_FilterRegionSize;
                numberofcolorsstr << (int) m_NumberOfColors;
                detectedradiusstr << m_DetectedRadius << " pixels";
                detectedanglestr << m_DetectedAngle.GetAngle(Angle::eAngleDegreePos) << " graus";
                detectionstatusstr << m_CenterMarkerFoundOnFrame + m_SpeedMarkerFoundOnFrame;
                absolutemovementstr << m_AbsoluteMovement;
                relativemovementstr << m_RelativeMovement;

                MagicColor();
                DrawText(10, 470, "Informacoes:");
                DrawText(10, 460, "Cor da marca central: " + centercolorstr.str());
                DrawText(10, 450, "Cor da marca posicional: " + speedcolorstr.str());
                DrawText(10, 440, "Posicao do marcador central: " + centermarkerstr.str());
                DrawText(10, 430, "Posicao do marcador posicional: " + speedmarkerstr.str());
                DrawText(10, 420, "Posicao do mouse: " + mouseposstr.str());
                DrawText(10, 410, "Tamanho da regiao do filtro: " + regionsizestr.str());
                DrawText(10, 400, "Tolerancia das cores: " + colorthresholdstr.str());
                DrawText(10, 390, "Raio detectado: " + detectedradiusstr.str());
                DrawText(10, 380, "Angulo detectado: " + detectedanglestr.str());
                DrawText(10, 370, "Numero de cores normalizcao: " + numberofcolorsstr.str());
                DrawText(10, 360, "Estado da deteccao (nro de pontos): " + detectionstatusstr.str());
                DrawText(10, 350, "Distancia absoluta: " + absolutemovementstr.str());
                DrawText(10, 340, "Distancia relativa: " + relativemovementstr.str());

                if(m_Logging)
                    DrawText(10, 330, "Informacoes de log: ligado");
                else 
                    DrawText(10, 330, "Informacoes de log: desligado");

                float r,g,b,a;

/*                glBegin(GL_POLYGON);
                m_CenterColor.ToGLFloat(r,g,b,a);
                glColor4f(r,g,b,a);
                glVertex2d(160, 470);
                glVertex2d(180, 470);
                glVertex2d(180, 460);
                glVertex2d(160, 460);
                glEnd();

                glBegin(GL_POLYGON);
                m_SpeedColor.ToGLFloat(r,g,b,a);
                glColor4f(r,g,b,a);
                glVertex2d(160, 460);
                glVertex2d(180, 460);
                glVertex2d(180, 450);
                glVertex2d(160, 450);
                glEnd();*/
            }
        }

        void CustomIdleFunction()
        {
            if(readFrame() > 0)
            {
                ParsePointsInfo();
                CalculateMovement();
                RenderFunction();
                LogStatus(m_Logging);
                ExportInfo();
                CleanUp();
            }
            else
            {
            }
        }

        bool CheckColor(int checkpos, int region, const Color& match, int tofound)
        {
            int founds = region * region;
            int xi, yi;
            ConvertToXY(checkpos, xi, yi);

            int xf = xi + 2*region;
            int yf = yi + 2*region;

            if(xi < 0) xi = 0;
            if(yi < 0) yi = 0;
            if(xf > 640) xf = 640;
            if(yf > 480) yf = 480;

            for(int x = xi; x < xf; ++x)
                for(int y = yi; y < yf; ++y)
                    if(getPixelColor(x,y).Match(match, m_ColorThreshHold))
                        --founds;

           return (founds <= region);
        }

        void ParsePointsInfo()
        {
            m_SpeedMarkerFoundOnFrame = 0;
            m_CenterMarkerFoundOnFrame = 0;

            vector<Point> centersfound;
            vector<Point> speedsfound;

            // Analysing image //
            int x, y;
            for(vector<int>::iterator it = m_SpeedsToCheck.begin(); it != m_SpeedsToCheck.end(); ++it)
                for(size_t i = 0; i < m_SpeedColor.size(); ++i)
                    if(CheckColor(*it, m_FilterAnalyzeRadius, m_SpeedColor[i], m_FilterAnalyzeRadius))
                    {
                        ConvertToXY(*it, x, y);
                        speedsfound.push_back(Point(x,y));
                    }

            for(vector<int>::iterator it = m_CentersToCheck.begin(); it != m_CentersToCheck.end(); ++it)
                for(size_t i = 0; i < m_CenterColor.size(); ++i)
                    if(CheckColor(*it, m_FilterAnalyzeRadius, m_CenterColor[i], m_FilterRegionSize))
                    {
                        ConvertToXY(*it, x, y);
                        centersfound.push_back(Point(x,y));
                    }

            if(centersfound.size())
            {
                int xsum = 0;
                int ysum = 0;

                for(size_t i = 0; i < centersfound.size(); ++i)
                {
                    xsum += centersfound[i].GetX();
                    ysum += centersfound[i].GetY();
                }

                m_LastCenterMarker = m_CenterMarker;
                m_CenterMarker.SetXY(xsum/centersfound.size(), ysum/centersfound.size());
                m_CenterMarkerFoundOnFrame = centersfound.size();
            }

            if(speedsfound.size())
            {
                int xsum = 0;
                int ysum = 0;

                for(size_t i = 0; i < speedsfound.size(); ++i)
                {
                    xsum += speedsfound[i].GetX();
                    ysum += speedsfound[i].GetY();
                }
                
                m_LastSpeedMarker = m_SpeedMarker;
                m_SpeedMarker.SetXY(xsum/speedsfound.size(), ysum/speedsfound.size());
                m_SpeedMarkerFoundOnFrame = speedsfound.size();
            }


            if(m_SpeedMarkerFoundOnFrame > 0 && m_CenterMarkerFoundOnFrame > 0 && m_RunTime.GetTime() != m_LastDetectionTime)
            {
                // Calculate the Radius and Angle of the Frame //
                Point pVect(m_SpeedMarker.GetX() - m_CenterMarker.GetX(), m_SpeedMarker.GetY() - m_CenterMarker.GetY());
                m_LastDetectedRadius = m_DetectedRadius;
                m_LastDetectedAngle = m_DetectedAngle;
                m_DetectedRadius = pVect.GetMag();
                m_DetectedAngle = pVect.GetAngle();
            }
        }

        void CalculateMovement()
        {
            if(m_SpeedMarkerFoundOnFrame > 0 && m_CenterMarkerFoundOnFrame > 0 && m_RunTime.GetTime() != m_LastDetectionTime)
            {
                int t = m_RunTime.GetTime();
                double passtime = double(t - m_LastDetectionTime); // miliseconds //

                double a = m_DetectedAngle.GetAngle(Angle::eAngleDegreePos);
                double la = m_LastDetectedAngle.GetAngle(Angle::eAngleDegreePos);
                double diff = 0;

                if(la >= a && la - a >= 180) // 359 -> 1 = 2 degrees
                    diff =  a + 360.0 - la;
                else if (la >= a && la - a < 180) // 170 -> 1 = -169 degrees
                    diff = a - la;
                else if (la < a && a - la >= 180) // 1 -> 359 = -2 degrees
                    diff = -(la + 360.0 - a);
                else if (la < a && a - la < 180) // 1 -> 170 = 169 degrees
                    diff = a - la;

                if(passtime > 0)
                    m_Speed = diff / passtime;
                else
                    m_Speed = 0;

                if(m_Speed > 100) m_Speed = 100;
                if(m_Speed < -100) m_Speed = -100;

                m_LastDetectionTime = m_RunTime.GetTime();
            }
        }

        void CustomMouseFunction(int button, int state, int x, int y)
        {
            m_Mouse.SetXY(x, 479 - y);

            if(button == GLUT_RIGHT_BUTTON && state == GLUT_DOWN)
            {
                m_Running = !m_Running;
                SkipReading(!m_Running);
            }

            if(button == GLUT_LEFT_BUTTON && state == GLUT_DOWN)
            {
                if(m_Mode == "CENTER")
                {
                    m_CenterColor.push_back(getPixelColor(x, 479 - y));
                    m_Mode = "";
                    m_ModeDesc = "";
                }
                else if(m_Mode == "SPEED")
                {
                    m_SpeedColor.push_back(getPixelColor(x, 479 - y));
                    m_Mode = "";
                    m_ModeDesc = "";
                }
                else if(m_Mode == "CAPTURE")
                {
                    double width = m_AnalysisRect.GetWidth() / 2;
                    double height = m_AnalysisRect.GetHeight() / 2;
                    m_AnalysisRect.SetBottomLeft(Point(x - width, 479-y - height));
                    m_AnalysisRect.SetTopRight(Point(x + width, 479-y + height));
                }
            }
        }

        void filter(int position, Color& color)
        {
            if(m_Normalize)
                color.Normalize();
            else if(m_NormalizeColors)
                color.NormalizeColors(m_NumberOfColors);

            if(position % m_FilterRegionSize == 0)
            {
                int x,y;
                ConvertToXY(position, x, y);
                if(Math::IsIn(Point(x,y), m_AnalysisRect))
                {
                    for(size_t i = 0; i < m_CenterColor.size(); ++i)
                        if(color.Match(m_CenterColor[i], m_ColorThreshHold))
                            m_CentersToCheck.push_back(position);

                    for(size_t i = 0; i < m_SpeedColor.size(); ++i)
                        if(color.Match(m_SpeedColor[i], m_ColorThreshHold))
                            m_SpeedsToCheck.push_back(position);
                }
            }
        }
};

int main(int argc, char* argv[])
{
    if(argc != 2)
    {
        TRACE("Invalid parameters number");
        TRACE("Use video as input device (not /dev/video0)");
        return -1;
    }

    TRACE("Forking application...");

    int pid = fork();
    string dev = argv[1];

    if(pid == 0) dev += "0";
    else dev += "1";

    TRACE("Selected device: " << dev);

    myVideoCapture app;

    TRACE("Initializing video capture application");
    app.Init(argc, argv);


    TRACE("Open " << dev << " device");
    if(!app.openVideoDevice(dev))
    {
        TRACE("Fail to open device");
        return -1;
    }

    TRACE("Initializing device");
    if(!app.initDevice())
    {
        TRACE("Fail to initialize device");
        return -2;
    }

    TRACE("Running MainLoop function");
    app.MainLoop();
}


